#include "BugTrackApp.h"
#include "RenderUtils.h"
#include "BugSimple.h"

float ambientColor[]= {0.3f, 0.3f, 0.3f, 1 };

BugTrackApp::BugTrackApp(int w, int h, int numWin)
{
	screenH = h; 
	screenW = w;
	
	this->tracking = false;
	this->steps = 0;
	this->dt = 0.025;
	this->numWindows = numWin;
	this->cams.SetNumCams(numWindows);
	Init();

	IplImage* tmp = cvCreateImage( cvSize(w*2,h*3),IPL_DEPTH_8U,1 );
	cvZero(tmp);

	if( ! GetSystemMetrics( SM_REMOTESESSION )  ) { // don't waste time when running on the node
		cvNamedWindow("RenderReadBackReal");
		cvShowImage( "RenderReadBackReal",tmp );
		cvMoveWindow("RenderReadBackReal",w*2,1);

		cvNamedWindow("RenderReadBackSynthetic");
		cvShowImage( "RenderReadBackSynthetic",tmp );
		cvMoveWindow("RenderReadBackSynthetic",1,h*3+20);

		cvNamedWindow(	"SegmentReal");
		cvShowImage( 	"SegmentReal",tmp );
		cvMoveWindow(	"SegmentReal",w*2,3*h+20);
	}


	cvReleaseImage(&tmp);
}

void BugTrackApp::IdleCallback() {

	// perform updates here!
	DisplayCallback();
}

char buffNum[8];
char buffNum2[8];
void BugTrackApp::Init()
{
	cout<<"Doing App Init...\n";
	bDrawRealNext		 = true;
	bRenderedSynthOnce	 = false;
	bRenderedRealOnce	 = false;
	for( int i = 0; i < this->numWindows; i++ ) {
		if( numWindows > 1 )
			glutSetWindow(i+1);
		glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
		glClearColor(0.0, 0.0 , 0.0, 0.0 );
		glShadeModel(GL_SMOOTH);
		glEnable(GL_DEPTH_TEST);
		glClear( GL_COLOR_BUFFER_BIT );
		glEnable(GL_DEPTH_TEST);
		glEnable(GL_LIGHTING);
		glEnable(GL_LIGHT0);
		glEnable(GL_LIGHT1);
		glEnable(GL_LIGHT2);
		glEnable(GL_LIGHT3);
		glEnable(GL_NORMALIZE);
		glEnable(GL_COLOR_MATERIAL);
	}
	if( numWindows > 1 )
		glutSetWindow(1);

	this->cams.InitCamsUniform(screenW, screenH);
	this->currentWindowID = 1;

	true_params_file = new fstream( "TrueBugParams.csv",ios::out );
	trck_params_file = new fstream( "TrackBugParams.csv",ios::out );

	// initialize the header line in these csv files...
	// step | bodyRx | bodyRy | bodyRz | bodyTx | bodyTy | bodyTz | ang1 ... | ang30 
	(*true_params_file)<<"timestep,bodyR0,bodyR1,bodyR2,bodyT0,bodyT1,bodyT2";
	(*trck_params_file)<<"timestep,bodyR0,bodyR1,bodyR2,bodyT0,bodyT1,bodyT2";
	for( int k = 1; k <= 6; k++ ) { // Note: 30 is the number of leg angles total! 5 angles, 6 legs
		for( int m = 1; m <= 5; m++ ) {
			_itoa_s(k,buffNum,8,10); // leg
			_itoa_s(m,buffNum2,8,10); // angle
			string angleName("Leg");
			angleName.append( string(buffNum) );
			angleName.append( string("angle") );
			angleName.append( string(buffNum2) );
			(*true_params_file)<<","<<angleName.c_str();
			(*trck_params_file)<<","<<angleName.c_str();
		}
	}
	(*true_params_file)<<"\n";
	(*trck_params_file)<<"\n";
	tracker.SetOutFile( this->trck_params_file );
}

BugTrackApp::~BugTrackApp()
{
	// delete any pointers here
	delete true_params_file; 
	delete trck_params_file;
}

void BugTrackApp::Reset() {
	// add any additional reset code here
	this->true_params_file->close();
	this->trck_params_file->close();
	delete true_params_file; 
	delete trck_params_file;
	Init();
}

// x & y are current mouse cursor coordinates
// x = 1, y = 1 is the upper left cornder of the window
void BugTrackApp::KeyboardCallback(unsigned char key, int x, int y)
{	
	if( key == 'R' ) {
		cout<<"Resetting... \n";
		this->Reset();
	}

	if( key == 'i' ) {
		cout<<"testing openCV...";
		IplImage* img = cvCreateImage( cvSize(300,300), IPL_DEPTH_8U, 3 );
		cvSaveImage( "test_img.png", img);
		cout<<" test_img.png has been saved! \n";
	}
	if( key == 't' ) {
		cout<<"opengl render readback and save...\n";
		//TestReadBuffer(this->screenH, this->screenW);
		SaveAllBuffers(screenW, screenH, numWindows, steps);
	}
	if( key == 's' ) {
		cout<<"saving real and tracked params and exitinng...\n";
		this->trck_params_file->close();
		this->true_params_file->close();
		delete true_params_file; 
		delete trck_params_file;
		char xx;
		cout<<"press enter...";
		cin>>xx;
		exit(0);
	}
	if (27 == key)
	{
		exit(0);

	}
}

#define BUFSIZE 512;
void BugTrackApp::MouseCallback(int button, int state, int x, int y)
{ 
   // use mouse click picking ? see old code somewhere for reference	
}


char buffParams[100];
void BugTrackApp::DisplayCallback()
{
	// from the tracker!!
	valarray<float> LegParams( 30 );
	valarray<float> bodT( 3 );
	valarray<float> bodR( 3 );

	// to check results from truth!
	valarray<float> outAng( 30 );
	valarray<float> outBodyT( 3 );
	valarray<float> outBodyR( 3 );

	if( numWindows > 1 ) {
		int idx = glutGetWindow()-1;
		glutSetWindow( currentWindowID ); // force-set it so clicking doens't change focus
	}
	
	if( !tracking ) {
		GenerateParams2Walking(steps * dt);
		if( 1 == currentWindowID ) {
			int ww,hh;
			this->cams.GetViewSize(currentWindowID-1,&ww,&hh);
			if( !GetSystemMetrics( 0x1000 ) );
				::SaveAllBuffers( ww,hh,this->numWindows,steps);
			steps++;
		}
	}
	else if( 1 == currentWindowID ) {
		GenerateParams2Walking(steps * dt);
		// write the true parameters to .csv file
		if( !tracker.IsInit() ) { // initialize at the first step 
			GetParams( outAng, outBodyT, outBodyR ); // get true params
			tracker.Init( this->numWindows, outBodyT, outBodyR, outAng );
			
		}
		// write the true & est. parameters to .csv file
		bDrawRealNext = tracker.IsConverged();
		if( bDrawRealNext && !bRenderedRealOnce) {
			GetParams( outAng, outBodyT, outBodyR ); // get true params
			(*true_params_file)<<steps<<","<<outBodyR[0]<<","<<outBodyR[1]<<","<<outBodyR[2];
			(*true_params_file)<<","<<outBodyT[0]<<","<<outBodyT[1]<<","<<outBodyT[2];
			for( int i = 0; i < outAng.size(); i++ ) {
				(*true_params_file)<<","<<outAng[i];
			}
			(*true_params_file)<<"\n";
		}

		// read-back the image, push it to the tracker
		if( bDrawRealNext ) {
			int ww,hh;
			if( bRenderedRealOnce ) {
				for( int k = 0; k < this->numWindows; k++ ) {
					cams.GetViewSize( k, &ww, &hh );
					CvMat* img = GetBuffer( ww,hh,k+1);
					tracker.AddNextRealView( img, k ); // flags tracker as not converged
				}
				GetParams( outAng, outBodyT, outBodyR ); // get true params
				tracker.SetTruth( outBodyT, outBodyR, outAng );
			}
			bRenderedSynthOnce = false;
			bRenderedRealOnce = true;
		}
	
		if( !bDrawRealNext ) {
			int ww,hh;
			if( bRenderedSynthOnce ) {
				for( int k = 0; k < this->numWindows; k++ ) {
					cams.GetViewSize( k, &ww, &hh );
					CvMat* img = GetBuffer( ww,hh,k+1);
					tracker.AddNextGenView( img, k ); // get the 'synthetic' rendered view
				}
				tracker.UpdateEstParams( bodT, bodR, LegParams );
				PassParams( LegParams, bodT, bodR ); // use this for letting tracker set parameters					
			}
			else {
				tracker.GetBestEstParams(bodT,bodR,LegParams);
				PassParams( LegParams, bodT, bodR ); // use this for letting tracker set parameters					
			}
			if( bRenderedRealOnce ) { // we just did the rendering, now update the step
				steps++;
			}
			bRenderedSynthOnce = true;
			bRenderedRealOnce = false;
		}


		

	}
	int NEnd = 10;
	if( steps == NEnd ) // stop at N frames 
		this->KeyboardCallback('s',0,0);

	cams.ModelView(currentWindowID-1); // Set CamView
	
	if( bDrawRealNext || !tracking)
		//drawBugModel();
		drawBug(); // do drawbug() if we're creating the 'rendered version'
	else
		drawBugModel(); // draw without background / lighting ('binary model')
		
	//TestGLRenderNoMV(); // Draw 
	//TestGLRender();
	
	if( currentWindowID == numWindows )
		cams.SetNumFrameDrawn(steps); // which frames num has actually been rendered?
	glutSwapBuffers();

	currentWindowID = currentWindowID % numWindows + 1;
	if( numWindows > 1 )
		glutSetWindow( currentWindowID );
}

void BugTrackApp::ResizeCallback(int w, int h)
{
	screenH = h;
	screenW = w;
	
	int idx = 0;
	if( numWindows > 1 )
		idx = glutGetWindow()-1;
	cams.ResizeCallbackOne(w,h,idx);
}


